#include "GetLidarData_LS72B.h"

GetLidarData_LS72B::GetLidarData_LS72B()
{
	all_lidardata.clear();
	all_lidardata_d.clear();
	Model = 0;
}

GetLidarData_LS72B::~GetLidarData_LS72B()
{
	deleteLidarData();
}

void GetLidarData_LS72B::onSerialPort(std::string com_name, int baud_rate)
{
	open(com_name.c_str(), baud_rate, 0, 8, 1);
}

void GetLidarData_LS72B::LidarStar()
{
	isQuit = false;
	std::thread t1(&GetLidarData_LS72B::receive, this);
	t1.detach();
}

bool GetLidarData_LS72B::open(const char* portname, int baudrate, char parity, char databit, char stopbit)
{
	fd = ::open(portname, O_RDWR | O_NOCTTY | O_NONBLOCK);
	if (fd == -1)
	{
		std::cerr << "Failed to open serial port" << std::endl;
		return false;
	}

	termios tty;
	memset(&tty, 0, sizeof tty);
	if (tcgetattr(fd, &tty) != 0)
	{
		std::cerr << "Error getting terminal attributes" << std::endl;
		close();
		return false;
	}

	cfsetospeed(&tty, baudrate);
	cfsetispeed(&tty, baudrate);

	tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity (most common)
	tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication (most common)
	tty.c_cflag &= ~CSIZE;  // Clear all bits that set the data size
	tty.c_cflag |= CS8;     // 8 bits per byte (most common)
	tty.c_cflag &= ~CRTSCTS;// Disable RTS/CTS hardware flow control (most common)
	tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)

	tty.c_lflag &= ~ICANON;
	tty.c_lflag &= ~ECHO; // Disable echo
	tty.c_lflag &= ~ECHOE; // Disable erasure
	tty.c_lflag &= ~ECHONL; // Disable new-line echo
	tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
	tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
	tty.c_iflag &= ~(ICRNL | INLCR); // Disable CR-to-NL, NL-to-CR
	tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
	tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed

	tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds)
	tty.c_cc[VMIN] = 0;

	if (tcsetattr(fd, TCSANOW, &tty) != 0)
	{
		std::cerr << "Error setting terminal attributes" << std::endl;
		close();
		return false;
	}

	return true;
}

std::string GetLidarData_LS72B::receive()
{
	std::string rec_str = "";
	char buf[100];
	DataTmpBuff.clear();

	while (true)
	{
		if (isQuit)
		{
			close();
			break;
		}

		ssize_t n = read(fd, buf, sizeof(buf));
		if (n < 0)
		{
			if (errno == EAGAIN)
			{
				usleep(10000); // Sleep for 10ms
				continue;
			}
			else
			{
				std::cerr << "Failed to read from serial port" << std::endl;
				close();
				return rec_str;
			}
		}

		DataTmpBuff.insert(DataTmpBuff.end(), buf, buf + n);
		// Process DataTmpBuff to extract Lidar data
		// ... (the rest of your logic remains the same)
	}
	return rec_str;
}

int GetLidarData_LS72B::send(char* dat, int datalong)
{
	ssize_t n = write(fd, dat, datalong);
	if (n < 0)
	{
		std::cerr << "Failed to write to serial port" << std::endl;
		return 0;
	}
	return n;
}

void GetLidarData_LS72B::close()
{
	if (fd != -1)
	{
		::close(fd);
		fd = -1;
	}
}

void GetLidarData_LS72B::LidarStop()
{
	isQuit = true;
}

void GetLidarData_LS72B::deleteLidarData()
{
	delete callback;
}

std::vector<MuchLidarData> GetLidarData_LS72B::getLidarPerFrameDate()
{
	isFrameOK = false;
	return LidarPerFrameDate;
}

void GetLidarData_LS72B::sendLidarData()
{
	if (Model == 0)
	{
		all_lidardata.insert(all_lidardata.end(), all_lidardata_d.begin(), all_lidardata_d.end());
		if (callback)
		{
			(*callback)(all_lidardata, 0);
		}
		LidarPerFrameDate = all_lidardata;
	}
	else if (Model == 1)
	{
		if (callback)
		{
			(*callback)(all_lidardata, 0);
		}
		LidarPerFrameDate = all_lidardata;
	}
	else if (Model == 2)
	{
		if (callback)
		{
			(*callback)(all_lidardata_d, 0);
		}
		LidarPerFrameDate = all_lidardata_d;
	}
	isFrameOK = true;
	all_lidardata.clear();
	all_lidardata_d.clear();
}

void GetLidarData_LS72B::readLidarState()
{
	// Lidar state logic implementation
}

std::string GetLidarData_LS72B::getLidarState()
{
	return strState;
}

void GetLidarData_LS72B::setLidarHz(u_int Hz)
{
	// Lidar Hz setting implementation
}